import numpy as np
import acado
import time
import matplotlib.pyplot as plt
from numpy import sin, cos,exp

import time

N=20 #timestep
NU=4
NOD=12
NX=10
NY=16
NYN=4

NW=16
NWN=12

x0=np.zeros(NX).reshape(1,10)
od=np.zeros(NOD)
y=np.zeros(NY)
u=np.zeros(NU)
Q=np.eye(NW)

Q=np.diag([1e-8,1e-8,1e-8,1,1,1,1,1,1,1,10,1000,1,50,50,50])


W=np.transpose(np.tile(Q,N))

print(W.shape)

WN=Q[:-4,:-4]

y=np.zeros([N,NY])

x0[0,1]=0.01
x0[0,2]=0.67
x0[0,3]=0.0

X=np.tile(x0.reshape(NX,1),N+1).T

od=np.array([6.0,0.0,0.67,10.0,0.0,0.0,1.0,0.0,1.0,10.0,1.0,1.0])
OD=np.tile(od.reshape(NOD,1),N+1).T


for i in range(N+1):
    od=np.array([4.0+0.1*i,0.0,0.67,10.0,0.0,0.0,1.0,0.0,1.0,10.0,1.0,1.0])
    OD[i,:]=od

    



u=np.zeros([N,NU])

xlog=[]

t0=time.time()

for i in range(200):
    print("iter: "+str(i))
    X,u=acado.mpc(0,1,x0,X,u,OD,y,W,WN,1)
    x0=X[1,:].reshape(1,10)
    
    xlog.append(x0)
    
xlog_arr=np.vstack(xlog)
t1=time.time()

print(t1-t0)


plt.subplot(221)
plt.plot(xlog_arr[:,0])
plt.subplot(222)
plt.plot(xlog_arr[:,1])

plt.show()
